#pragma once

#include "mbt/tracker.hh"

class BundleHist;

class RBTracker : public TrackerBase {
public:
	RBTracker(const cv::Matx33f& K, std::vector<Object3D*>& objects);
	~RBTracker();

	virtual void ToggleTracking(cv::Mat& frame, int objectIndex, bool undistortFrame = true) override;
	virtual void EstimatePoses(cv::Mat& frame, bool check_lost) override;
	virtual void UpdateHist(cv::Mat& frame) override;

protected:
	virtual void Track(std::vector<cv::Mat>& imagePyramid, std::vector<Object3D*>& objects, int runs = 1) override;
	void RunIteration(std::vector<Object3D*>& objects, const std::vector<cv::Mat>& imagePyramid, int level);
	void ComputeJacobians(Object3D* object, const cv::Mat& frame, const cv::Mat& depth, const cv::Mat& depthInv, const cv::Mat& sdt, const cv::Mat& xyPos, const cv::Rect& roi, const cv::Mat& mask, int m_id, int level, cv::Matx66f& wJTJ, cv::Matx61f& JT);

	SignedDistanceTransform2D* SDT2D;
};